#include "MainSense.hh"
#include "OnlineCamera.hh"
#include "SimulatedCamera.hh"
#include "MyAction.hh"
#include <iostream>

using namespace sense;
using namespace camera;


void help(int argc, char** argv) {
	std::cerr << std::endl
		<< "instructions:  " << argv[0] << " -cf <main/global.cfg>"
		<< std::endl << std::endl;
}

int main(int argc, char* argv[]) {
	std::string cfMain;
	std::string cfSense = "";
	std::string cfCamera = "";
	std::string cfAction = "";
	bool simulation;

	int argi = 1;
	while (argi < argc) {
		if (std::string(argv[argi]) == "-h" ||
			std::string(argv[argi]) == "--help") {
			help(argc, argv);
			return -1;
		} 
		else if (std::string(argv[argi]) == "-cf") {
			cfMain = argv[++argi];
		}
		else {
		  help(argc, argv);
		  return -1;
		}
		++argi;
	}

	util::ConfigFile cf(cfMain);
	simulation = cf.getBool("SIMULATION", false);
	cfSense = cf.getString("SENSE_CONFIG_FILE");
	cfCamera = cf.getString("CAMERA_CONFIG_FILE");
	cfAction = cf.getString("ACTION_CONFIG_FILE");

	try {

		Sense *sense = new MainSense(cfSense);
		sense->start();

    // If the camera is simulated, SimulatedCamera is used instead 
    // of the usual OnlineCamera that reads images from the real IP Camera 
		Camera *camera;
		if (simulation) {
			camera = new SimulatedCamera(cfCamera);
		} else {
			camera = new OnlineCamera(cfCamera);
		}
		camera->start();

		Action *action = new MyAction(sense, camera, cfAction);
	//	sleep(10);
		action->start();

		std::cerr << "Robot ready" << std::endl;

		char command = 0;
		// press q to quit
		while (command != 'q') {
			std::cin.get(command);
		}

		std::cerr << "Stopping robot" << std::endl;

    sleep(2);
    
		action->stop();
		delete action;

		sense->stop();
		delete sense;

		camera->stop();
		delete camera;


	} catch (std::exception& error) {
		// what happened wrong?
		std::cerr << error.what() << std::endl;
	}

	return 0;
}
